#include "Calibrate.h"

// uint8_t StSEBuf[20] = {0};
uint8_t St1_Buf[20] = {0};
Sentence_t StSE = StCreate("SE:",',',"\n",St1_Buf,R5_Buff);
Sentence_t StPID = StCreate("PID:", ',', "\n", St1_Buf, R1_Buff);
Sentence_t StMC = StCreate("MC:", ',', "\n", St1_Buf, R5_Buff);
Sentence_t StParam = StCreate("Param:", ',', "\n", St1_Buf, R5_Buff);

CaliCtrl_t CaliC=
{
    .mode = 0,
    .status = 0,
    .SE_Enable = 0,
	.PID_Enable = 0,
    .Motor_Enable = 0,
    .Param_Enable = 0,
};

void SE_Calibrate(void)
{
	if(St_SPLIT(&StSE))
	{
        switch(*St_GetStr(&StSE,0))
        {
            case 'o':CaliC.SE_Enable = St_GetInt(&StSE,1) != DISABLE ? ENABLE:DISABLE;break;
            case 'e':hSE1.Enable = St_GetInt(&StSE,1) != 0 ? 1:0;break;
            case 'a':hSE1.Max = St_GetInt(&StSE,1);break;
            case 'i':hSE1.Min = St_GetInt(&StSE,1);
                     hSE1.Max = hSE1.Min - 500;break;
            case 't':SE1_Set_Timer(St_GetInt(&StSE,1),St_GetInt(&StSE,1),1);break;
            case 's':systemConfigInfo.SE_Max = hSE1.Max;
                     systemConfigInfo.SE_Min = hSE1.Min;
                     WS_Config_System_Save();//保存到Flash
                     Beep(100);break;
        }
        Vofa_Draw("SE","%d,%d,%d,%d,%d,%d",CaliC.SE_Enable,hSE1.Enable,hSE1.Max,hSE1.Min,hSE1.duty2,(int)(hSE1.pressure*1000));

	}
	
}

int PID_Prase(void)
{
	if(St_SPLIT(&StPID))
	{
        switch(*St_GetStr(&StPID,0))
        {
			case 'o': CaliC.PID_Enable = St_GetInt(&StPID,1) != DISABLE ? ENABLE:DISABLE;break;
            case 'p': pidX.Kp = St_GetDob(&StPID,1)/1E3;break;
            case 'i': pidX.Ki = St_GetDob(&StPID,1)/1E3;break;
            case 'd': pidX.Kd = St_GetDob(&StPID,1)/1E3;break;
            case 'a': pidX.Kp = St_GetDob(&StPID,1)/1E3;
		              pidX.Ki = St_GetDob(&StPID,2)/1E3;
		              pidX.Kd = St_GetDob(&StPID,3)/1E3;break;
        }
        UARTx_Printf(&huart1,VofaDataFormat("PID","%.4f,%.4f,%.4f,%.4f",MC.pidOut,pidX.Kp,pidX.Ki,pidX.Kd));
		Beep(30);
        return 1;
	}
	else return 0;
	
}

int MC_Prase(void)
{
    static uint16_t x=0,y=0;
	if(St_SPLIT(&StMC))
	{
        switch(*St_GetStr(&StMC,0))
        {
            case 'o': CaliC.Motor_Enable = St_GetInt(&StMC,1) != DISABLE ? ENABLE:DISABLE;break;
            case 'x': x = St_GetInt(&StMC,1);break;
            case 'y': y = St_GetInt(&StMC,1);break;
            case 'g': MC_Set_ModeState(Order,ToAim);//GoAim
                      Motor_GoAim_Start(x,y);
                      break;
            case 'p': MC_Set_ModeState(Order,ToPick);//Pick
                      MC_Pick_Start(x,y);
                      break;
            case 'u': MC_Set_ModeState(Order,ToUnload);//Unlode
                      MC_Unload_Start(x,y);
                      break;
            case 'r': MC_Set_ModeState(Order,Rst);//Reset
                      Motor_Mode_Start(RST);
                      break;
            case 't': 
                    //   CamC_Start_StaticTrack();
                      MC_Pick_Start(x,y);
                      MC_Set_ModeState(Order,CamTrack);//采摘校准
                      break;
            case 's': MC_Set_ModeState(Order,ToAim);//Stop
                      Motor_GoAim_Start(MC.MapX,MC.MapY);
                      MotorZ_Set_Speed(MotorZ_STOP_SPEED);
                      break;
            case 'v': if(St_CmpStr(&StMC,1,"X"))//MC:v,X,100\n
                      {
                        MotorX.uSpeed = St_GetInt(&StMC,2);
                        MotorX.dSpeed = -St_GetInt(&StMC,2);
                      }
                      else if(St_CmpStr(&StMC,1,"Y"))//MC:v,Y,100\n
                      {
                        MotorX.uSpeed = St_GetInt(&StMC,2);
                        MotorY.dSpeed = -St_GetInt(&StMC,2);
                      }
                      DEBUG("MotorX Speed (%d,%d)",MotorX.uSpeed,MotorX.dSpeed);
                      DEBUG("MotorY Speed (%d,%d)",MotorY.uSpeed,MotorY.dSpeed);
                      break;
            case 'c':
                      if(St_CmpStr(&StMC,1,"Encode"))//MC:c,Encode[\n]
                      {
                          MC.XEncode++;
                          if(MC.XEncode > ENCODE_Stepper)MC.XEncode = ENCODE_ME;
                      }
                      else if(St_CmpStr(&StMC,1,"ADChip"))//MC:c,ADChip[\n]
                      {
                          CA.ADSChip = !CA.ADSChip;//切换电容芯片
                      }
                      else if(St_CmpStr(&StMC,1,"XCalibrate"))//MC:c,XCalibrate[\n]
                      {
                          MC.XEncode = ENCODE_ME;
                          MC_Set_ModeState(Calibrate,Ready);//开始校准
                          DEBUGS("[XCalibrate Start!]\n\r");
                      }
                      else if(St_CmpStr(&StMC,1,"SLRStart"))//MC:c,SLRStart[\n]
                      {
                          SLR_Calibrate();
                          DEBUG("SLR Start Point is [%d]\n\r",SLR.Start);
                      }
                      else if(St_CmpStr(&StMC,1,"Save"))//MC:c,Save[\n]
                      {
                          systemConfigInfo.CA_ADSChip = CA.ADSChip;
                          systemConfigInfo.MC_XEncode = MC.XEncode;
                          systemConfigInfo.SLR_Start = SLR.Start;
                          WS_Config_System_Save();
                          DEBUGS("[Param Saved!]\n\r");
                          Beep(500);
                      }
                      else if(St_CmpStr(&StMC,1,"unSave"))//MC:c,unSave[\n]
                      {
                          CA.ADSChip = systemConfigInfo.CA_ADSChip;
                          SLR.Start = systemConfigInfo.SLR_Start;
                          MC.XEncode = systemConfigInfo.MC_XEncode;//不保存
                          DEBUGS("[Parameter restoration succeeded]\n\r");
                          Beep(100);
                      }
                      DEBUG(" [XEncode]   {%s}\n\r",ENCODE_NAME[MC.XEncode]);
                      DEBUG(" [ADSChip]   {%s}\n\r",ADSCHIP_NAME[CA.ADSChip]);
                      DEBUG(" [Motor]     {%s}\n\r",Motor_Name[MotorX_SELECT]);
                      break;
            
        }
        MC_MapFlash();
        Vofa_Draw("MapPos","%d,%d",MC.MapX,MC.MapY);
        Beep(30);
        	
		return 1;
	}
	else return 0;
	

}

int Param_Prase(void)
{
	if(St_SPLIT(&StParam))
	{
        char* str = St_GetStr(&StParam,0);
        switch(*str)
        {
			case 'o': CaliC.Param_Enable = St_GetInt(&StParam,1) != DISABLE ? ENABLE:DISABLE;break;
            case 'K': //Param:KLxxx,Get
                if(str[1] == 'L')Kalman_Param(str);break;
            case 'R': SEKF.r = St_GetDob(&StParam,1)/1E4;break;
            case 'C': SEKF.x=0;SEKF.p=0;SEKF.K=0;break;
        }
        if(strcmp("offset",str) == 0)
        {
            if(*St_GetStr(&StParam,1) == 'X')
            {
                CamC.offsetX = St_GetInt(&StParam,2);//Param:offset,X,5[\n]
            }
            if(*St_GetStr(&StParam,1) == 'Y')
            {
                CamC.offsetY = St_GetInt(&StParam,2);//Param:offset,Y,5
            }
            DEBUG("return >> Cam Offset is (%d,%d)\n\r",CamC.offsetX,CamC.offsetY);
        }
        Beep(30);
        // Vofa_Printf("Param >> %f,%f\n\r",SEKF.q,SEKF.r);
		return 1;
	}
	else return 0;
	
}

void Cali_Run(void)
{
	if(CaliC.SE_Enable)
	{
		Vofa_Draw("SE","%d,%d,%d,%d,%d,%d,%d",CaliC.SE_Enable,hSE1.Enable,\
        hSE1.Max,hSE1.Min,hSE1.duty2,(int)(hSE1.pressure*1000),(int)(hSE1.pressure*1000));
	}
    if(CaliC.PID_Enable)
    {
        UARTx_Printf(&huart1,VofaDataFormat("PID","%.4f,%.4f,%.4f,%.4f,%d",MC.pidOut,pidX.Kp,pidX.Ki,pidX.Kd,CamC.Rdata->x));
    }
    if(CaliC.Motor_Enable)
    {
        MC_MapFlash();
        Vofa_Draw("Motor","%d,%d,%d,%d,%d",MC.MapX,MC.MapY,CA.Cap,SLR.filterData,XStepToLength(MotorX.step_cnt));
    }
    if(CaliC.Param_Enable)
    {
        Vofa_Draw("Param","%f,%f",SEKF.q,SEKF.r);
    }
}

void Kalman_Param(char* str)
{
    if(strcmp(str,"KLSLR") == 0)Kalman_Cali_CMD(&SLRFilter);

}

void Kalman_Cali_CMD(kalman1_t* pKL)
{
    char* str = St_GetStr(&StParam, 1);
    switch(*str)
    {
        case 'G':
                if(str[1] =='e' && str[2] =='t')
                    DEBUG("KalmanParam Q(%lf),R(%lf),X(%lf)\n",pKL->q,pKL->r,pKL->x);
                break;
        case 'Q':pKL->q = St_GetDob(&StParam,2); break;
        case 'R':pKL->r = St_GetDob(&StParam,2); break;
        case 'C':pKL->x = 0;pKL->q = 0;pKL->r = 0;break;
        case 'A':pKL->q = St_GetDob(&StParam,2);
                 pKL->r = St_GetDob(&StParam,3);
                 break;
    }
}


